#include <Servo.h>

// motors
const int E1 = 5; //M1 Speed Control
const int E2 = 6; //M2 Speed Control
const int M1 = 4; //M1 Direction Control
const int M2 = 7; //M1 Direction Control

// buttons
const int key_S6 = 2, key_S7 = 3, key_S1_5 = 7;

// servos of turrel
Servo servo_turrel_low, servo_turrel_high;

// IF Range Switch
const int infra_front_left = 10;
const int infra_front_right = 9;
const int infra_back_left = 11;
const int infra_back_right = 8;

///////////////
// moteur
void motor_stop(void) //Stop
{
  digitalWrite(E1,LOW);
  digitalWrite(E2,LOW);
}
void motor_move_front(char a,char b) //Move forward
{
  analogWrite (E1,a); //PWM Speed Control
  digitalWrite(M1,HIGH);
  analogWrite (E2,b);
  digitalWrite(M2,HIGH);
}
void motor_move_back(char a,char b) //Move backward
{
  analogWrite (E1,a);
  digitalWrite(M1,LOW);
  analogWrite (E2,b);
  digitalWrite(M2,LOW);
}
void motor_turn_right (char a,char b) //Turn Right
{
  analogWrite (E1,a);
  digitalWrite(M1,LOW);
  analogWrite (E2,b);
  digitalWrite(M2,HIGH);
}
void motor_turn_left (char a,char b) //Turn Left
{
  analogWrite (E1,a);
  digitalWrite(M1,HIGH);
  analogWrite (E2,b);
  digitalWrite(M2,LOW);
}
void setup_motor(void)
{
  pinMode(E1, OUTPUT);
  pinMode(E2, OUTPUT);
  pinMode(M1, OUTPUT);
  pinMode(M2, OUTPUT);
}

//////////////////
// boutons
int check_buttons()
{
  // Cette fonction vérifie les boutons et retourne
  // 0 si pas de bouton appuyé ou sinon 1 à 7 suivant S1...S7
  // Priorités : S7, S6, S1, S2, S3, S4, S5
  if (digitalRead(key_S7)==0) return 7;
  if (digitalRead(key_S6)==0) return 6;
  int w = analogRead(key_S1_5);
#define vS1 0
#define vS2 130
#define vS3 306
#define vS4 478
#define vS5 720
  if ( w < vS2/2 ) return 1;
  if ( w < (vS3+vS2)/2 ) return 2;
  if ( w < (vS4+vS3)/2 ) return 3;
  if ( w < (vS5+vS4)/2 ) return 4;
  if ( w < (1024+vS5)/2 ) return 5;
  return 0;
}
void setup_buttons(){
  pinMode(key_S6, INPUT);
  pinMode(key_S7, INPUT);
}

//////////////
// tourelle
const int turrel_low_min = 10, turrel_low_mid = 80, turrel_low_max = 170;
const int turrel_high_min = 30, turrel_high_mid = 85, turrel_high_max = 105;
int current_turrel_position_low = turrel_low_mid;
int current_turrel_position_high = turrel_high_mid;

void test_servo_turrel(Servo svo, int rang_min=0, int rang_mid=90, int rang_max=180, int dlay=15)
{
  int pos;
  for(pos = rang_mid; pos < rang_max; pos += 1)
  {
    svo.write(pos);
    delay(dlay);
  }
  for(pos = rang_max; pos>=rang_min; pos-=1)
  {                                
    svo.write(pos);
    delay(dlay);
  }
  for(pos = rang_min; pos < rang_mid; pos += 1)
  {
    svo.write(pos);
    delay(dlay);
  }
}
void reset_turrel_position(int dlay=15)
{
  current_turrel_position_low = turrel_low_mid;
  current_turrel_position_high = turrel_high_mid;
  servo_turrel_low.write(turrel_low_mid); 
  servo_turrel_high.write(turrel_high_mid);
  delay(dlay);    
}
void test_turrel_low()
{
  test_servo_turrel(servo_turrel_low, turrel_low_min, turrel_low_mid, turrel_low_max);
}
void test_turrel_high()
{
  test_servo_turrel(servo_turrel_high, turrel_high_min, turrel_high_mid, turrel_high_max);
}
void set_turrel_position(int low, int high)
{
  int set_low = (low<turrel_low_min)?turrel_low_min:(low>turrel_low_max)?turrel_low_max:low;
  int set_high = (high<turrel_high_min)?turrel_high_min:(high>turrel_high_max)?turrel_high_max:high;
  //  int cur_low = servo_turrel_low.read();
  //  int cur_high = servo_turrel_high.read();
  int dlay=15;
  servo_turrel_low.write(set_low); 
  servo_turrel_high.write(set_high);
  current_turrel_position_low = set_low;
  current_turrel_position_high = set_high;
  delay(dlay);
}
void acc_turrel_position(int low, int high)
{
  set_turrel_position(current_turrel_position_low + low, current_turrel_position_high + high);
}

void setup_turrel()
{
  servo_turrel_low.attach(12);
  servo_turrel_high.attach(13);
  reset_turrel_position(50);
}

//////////////
// IF Range Swich
int setup_rangeswiches(){
  pinMode(infra_front_left, INPUT);
  pinMode(infra_front_right, INPUT);
  pinMode(infra_back_left, INPUT);
  pinMode(infra_back_right, INPUT);
}

//////////////
// main
void setup()
{
  Serial.begin(9600); //Set Baud Rate
  setup_motor();
  setup_buttons();
  setup_rangeswiches();
  setup_turrel();  
}
int check_sensors()
{
  char val = 0;
  boolean fl = (digitalRead(infra_front_left)==LOW)?true:false;
  boolean fr = (digitalRead(infra_front_right)==LOW)?true:false;
  boolean bl = (digitalRead(infra_back_left)==LOW)?true:false;
  boolean br = (digitalRead(infra_back_right)==LOW)?true:false;
  //    boolean fl=false, fr=false, bl=false, br = false;
  if (fl && fr && bl && br) val = 0; // dont move : cerned !
  else if(fl && fr) val = 2; // go back !
  else if(bl && br) val = 4; // go front !
  if(val == 0)
  {
    if(fl || br) val = 1; // rotate left
    else if(fr || bl) val = 3; // rotate right
  }

  return val>0?8:0;
}
void send_sensors_status()
{
  boolean fl = (digitalRead(infra_front_left)==HIGH)?true:false;
  boolean fr = (digitalRead(infra_front_right)==HIGH)?true:false;
  boolean bl = (digitalRead(infra_back_left)==HIGH)?true:false;
  boolean br = (digitalRead(infra_back_right)==HIGH)?true:false;

  Serial.print("fl="); Serial.print(fl?"0":"1"); Serial.print(";");
  Serial.print("fr="); Serial.print(fr?"0":"1"); Serial.print(";");
  Serial.print("bl="); Serial.print(bl?"0":"1"); Serial.print(";");
  Serial.print("br="); Serial.print(br?"0":"1"); Serial.println(";");
}

void loop(void)
{
  char val = check_sensors();
  if(val == 0)
    val = check_buttons();
  if((val == 0 || val == 8) && Serial.available())
    val = Serial.read();

  if(val>0)
  {
    switch(val)
    {
    case 'q'://Turn Left
    case 1:
      motor_turn_left(150,150);
      break;
    case 's'://Move back
    case 2:
      motor_move_back(100,100);
      break;
    case 'd'://Turn Right
    case 3:
      motor_turn_right (150,150);
      break;
    case 'z'://Move front
    case 4:
      motor_move_front(100,100);
      break;
    case 5: // reset turrel position
    case 'r':
      motor_stop();
      reset_turrel_position();
      break;
    case 6: // test turrel low
    case 'u':
      test_turrel_low();
      break;
    case 7: // test turrel high
    case 'o':
      test_turrel_high();
      break;
    case 'i': // set turrel high -1 
      acc_turrel_position(0, -1);
      break;
    case 'k': // set turrel high +1
      acc_turrel_position(0, +1);
      break;
    case 'j': // set turrel low +1 
      acc_turrel_position(1, 0);
      break;
    case 'l': // set turrel low -1
      acc_turrel_position(-1, 0);
      break;
    case 'g': // get sensors status
      send_sensors_status();
      break;
    case 8:
    default:
      motor_stop();
      break;
    }
  }
}






